﻿// <copyright file="HandTrackingService.cs" company="Ucaya">
// </copyright>
// <author>Ucaya</author>

namespace KinectExtensions.Services
{
    #region

    using System;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.ComponentModel;
    using System.ComponentModel.Composition;
    using System.Linq;
    using System.Linq.Expressions;
    using System.Windows;

    using KinectExtensions.Extensions;
    using KinectExtensions.Helpers;
    using KinectExtensions.Nui;

    using Microsoft.Research.Kinect.Nui;

    #endregion

    /// <summary>
    ///   TODO: Update summary.
    /// </summary>
    [Export]
    public class HandTrackingService : INotifyPropertyChanged
    {
        #region Constants and Fields

        /// <summary>
        ///   The hand position sync lock.
        /// </summary>
        private readonly object handPositionSyncLock = new object();

        /// <summary>
        ///   The hand master.
        /// </summary>
        private HandPosition handMaster;

        /// <summary>
        ///   The last hand master position.
        /// </summary>
        private Point? lastHandMasterPosition;

        #endregion

        #region Constructors and Destructors

        /// <summary>
        ///   Initializes a new instance of the <see cref = "HandTrackingService" /> class.
        /// </summary>
        public HandTrackingService()
        {
            ServiceLocator.Instance.SatisfyImportsOnce(this);
            this.HandPositions = new ObservableCollection<HandPosition>();
            this.KinectService.KinectRuntime.SkeletonFrameReady += this.KinectRuntime_SkeletonFrameReady;
            this.KinectService.InteractionManager.CurrentInteractionModeChanged += this.InteractionManager_CurrentInteractionModeChanged;

            this.HelloGestureTracker = new HelloGestureTracker();
            this.HelloGestureTracker.HelloGestureDetected += this.helloTracker_HelloGestureDetected;
        }

        #endregion

        #region Public Events

        /// <summary>
        ///   The hand positions changed.
        /// </summary>
        public event EventHandler<HandPositionChangedEventArgs> HandPositionsChanged;

        /// <summary>
        ///   The master position changed.
        /// </summary>
        public event EventHandler<RelativePositionEventArgs> MasterPositionChanged;

        /// <summary>
        ///   The property changed.
        /// </summary>
        public event PropertyChangedEventHandler PropertyChanged;

        #endregion

        #region Public Properties

        /// <summary>
        ///   Gets or sets HandMaster.
        /// </summary>
        public HandPosition HandMaster
        {
            get
            {
                return this.handMaster;
            }

            set
            {
                if (this.handMaster != value)
                {
                    if (this.handMaster != null)
                    {
                        this.handMaster.IsMaster = false;
                        this.OnMasterPositionChanged(null);
                    }

                    this.handMaster = value;

                    if (this.handMaster != null)
                    {
                        this.handMaster.IsMaster = true;
                    }

                    this.RaisePropertyChanged(n => n.HandMaster);
                }
            }
        }

        /// <summary>
        ///   Gets HandPositions.
        /// </summary>
        public ObservableCollection<HandPosition> HandPositions { get; private set; }

        public HelloGestureTracker HelloGestureTracker { get; private set; }

        /// <summary>
        ///   Gets or sets KinectService.
        /// </summary>
        [Import]
        public KinectService KinectService { get; set; }

        #endregion

        #region Methods

        /// <summary>
        ///   The on hand positions changed.
        /// </summary>
        /// <param name = "positions">
        ///   The positions.
        /// </param>
        protected virtual void OnHandPositionsChanged(HandPosition[] positions)
        {
            if (this.HandPositionsChanged != null)
            {
                this.HandPositionsChanged(this, new HandPositionChangedEventArgs(positions));
            }
        }

        /// <summary>
        ///   The on master position changed.
        /// </summary>
        /// <param name = "position">
        ///   The position.
        /// </param>
        protected virtual void OnMasterPositionChanged(Point? position)
        {
            if (this.MasterPositionChanged != null)
            {
                this.MasterPositionChanged(this, new RelativePositionEventArgs(position));
            }
        }

        /// <summary>
        ///   The raise property changed.
        /// </summary>
        /// <param name = "expression">
        ///   The expression.
        /// </param>
        /// <typeparam name = "TProperty">
        /// </typeparam>
        protected void RaisePropertyChanged<TProperty>(Expression<Func<HandTrackingService, TProperty>> expression)
        {
            if (this.PropertyChanged != null)
            {
                this.PropertyChanged(this, new PropertyChangedEventArgs(this.GetPropertyName(expression)));
            }
        }

        private void InteractionManager_CurrentInteractionModeChanged(object sender, EventArgs e)
        {
            this.HelloGestureTracker.IsEnabled = this.KinectService.InteractionManager.CurrentInteractionMode != Interactions.Kinect;
        }

        /// <summary>
        ///   The kinect runtime_ skeleton frame ready.
        /// </summary>
        /// <param name = "sender">
        ///   The sender.
        /// </param>
        /// <param name = "e">
        ///   The e.
        /// </param>
        private void KinectRuntime_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            if (this.HelloGestureTracker.IsEnabled)
            {
                SkeletonData skeletonData = e.SkeletonFrame.Skeletons.FirstOrDefault(_ => _.TrackingState == SkeletonTrackingState.Tracked);
                if (skeletonData == null)
                {
                    return;
                }

                Joint handLeft = skeletonData.Joints[JointID.HandLeft];
                Joint handRight = skeletonData.Joints[JointID.HandRight];
                Joint hipCenter = skeletonData.Joints[JointID.HipCenter];

                Joint? activeHand = (from h in new[] { handLeft, handRight }
                                     where h.Position.Y > hipCenter.Position.Y
                                     orderby h.Position.Y descending
                                     select (Joint?)h).FirstOrDefault();

                this.HelloGestureTracker.IsHandReady = activeHand != null;
                if (activeHand != null)
                {
                    this.PushTrackedHandMovementToGestureService(skeletonData, activeHand.Value);
                }
            }

            if (!this.KinectService.IsEnabled)
            {
                this.HandPositions.Clear();
                this.HandMaster = null;
                return;
            }

            var changedPosition = new List<HandPosition>();

            lock (this.handPositionSyncLock)
            {
                foreach (var skeletonData in e.SkeletonFrame.Skeletons)
                {
                    SkeletonData data = skeletonData;
                    HandPosition[] handPositions = this.HandPositions.Where(_ => _.TrackingId == data.TrackingID).ToArray();
                    if (skeletonData.TrackingState != SkeletonTrackingState.Tracked)
                    {
                        foreach (var handPosition in handPositions)
                        {
                            changedPosition.Add(handPosition);
                            var newJoint = new Joint { ID = handPosition.JointId, TrackingState = JointTrackingState.NotTracked };
                            handPosition.Joint = newJoint;
                            changedPosition.Add(handPosition);
                            this.HandPositions.Remove(handPosition);
                        }
                    }
                    else if (skeletonData.TrackingState == SkeletonTrackingState.Tracked)
                    {
                        HandPosition handLeft;
                        HandPosition handRight;
                        if (handPositions.Length != 0)
                        {
                            handRight = handPositions.First(_ => _.JointId == JointID.HandRight);
                            handLeft = handPositions.First(_ => _.JointId == JointID.HandLeft);
                        }
                        else
                        {
                            handRight = new HandPosition { TrackingId = skeletonData.TrackingID, JointId = JointID.HandRight };
                            handLeft = new HandPosition { TrackingId = skeletonData.TrackingID, JointId = JointID.HandLeft };
                        }

                        handLeft.TimeStamp = e.SkeletonFrame.TimeStamp;
                        handRight.TimeStamp = e.SkeletonFrame.TimeStamp;

                        Joint handRightJoint = skeletonData.Joints[JointID.HandRight];
                        Joint handLeftJoint = skeletonData.Joints[JointID.HandLeft];
                        if (handRightJoint.Position.W < 0.7 || handLeftJoint.Position.W < 0.7 || float.IsInfinity(handRightJoint.Position.X) || float.IsInfinity(handRightJoint.Position.Y) || float.IsInfinity(handLeftJoint.Position.X) || float.IsInfinity(handLeftJoint.Position.Y))
                        {
                            continue;
                        }

                        // Set positions from hip center. Seems to be the most unaffected point when moving
                        Joint hipJoint = skeletonData.Joints[JointID.HipCenter];
                        Joint headJoint = skeletonData.Joints[JointID.Head];
                        float referenceSizeX = (headJoint.Position.Y - hipJoint.Position.Y) * 0.8f;
                        float referenceSizeY = referenceSizeX * 0.5f;

                        float rightHandXOrigin = hipJoint.Position.X;
                        float rightHandYOrigin = hipJoint.Position.Y + referenceSizeX * 0.2f;

                        handRightJoint = handRightJoint.Move(x =>
                            {
                                return (x - rightHandXOrigin) / referenceSizeX;
                            }, y =>
                                {
                                    return 1 - (y - rightHandYOrigin) / referenceSizeY;
                                });

                        float leftHandXOrigin = hipJoint.Position.X - referenceSizeX;
                        float leftHandYOrigin = rightHandYOrigin;
                        handLeftJoint = handLeftJoint.Move(x =>
                            {
                                return (x - leftHandXOrigin) / referenceSizeX;
                            }, y =>
                                {
                                    return 1 - (y - leftHandYOrigin) / referenceSizeY;
                                });

                        Point pt = handRight.RoundPoint(new Point(handRightJoint.Position.X, handRightJoint.Position.Y));
                        if (pt != handRight.Point)
                        {
                            changedPosition.Add(handRight);
                            handRight.Point = pt;
                        }

                        handRight.Joint = skeletonData.Joints[JointID.HandRight];

                        pt = handLeft.RoundPoint(new Point(handLeftJoint.Position.X, handLeftJoint.Position.Y));
                        if (pt != handLeft.Point)
                        {
                            changedPosition.Add(handLeft);
                            handLeft.Point = pt;
                        }

                        handLeft.Joint = skeletonData.Joints[JointID.HandLeft];

                        this.SetHandMaster(handRight, skeletonData, handLeft);

                        if (handPositions.Length == 0)
                        {
                            this.HandPositions.Add(handRight);
                            this.HandPositions.Add(handLeft);
                        }
                    }
                }
            }

            if (changedPosition.Count > 0)
            {
                this.OnHandPositionsChanged(changedPosition.ToArray());
            }
        }

        private void PushTrackedHandMovementToGestureService(SkeletonData skeletonData, Joint hl)
        {
            if (this.HelloGestureTracker.GestureService == null || this.HelloGestureTracker.GestureService.JointId != hl.ID)
            {
                this.HelloGestureTracker.GestureService = (skeletonData.GetJointGestureService(hl.ID));
            }

            this.HelloGestureTracker.GestureService.AddPosition(skeletonData);
        }

        /// <summary>
        ///   The set hand master.
        /// </summary>
        /// <param name = "handRight">
        ///   The hand right.
        /// </param>
        /// <param name = "skeletonData">
        ///   The skeleton data.
        /// </param>
        /// <param name = "handLeft">
        ///   The hand left.
        /// </param>
        private void SetHandMaster(HandPosition handRight, SkeletonData skeletonData, HandPosition handLeft)
        {
            if (this.HandMaster == null || this.HandMaster.TrackingId == skeletonData.TrackingID)
            {
                IOrderedEnumerable<HandPosition> master = from hand in new[] { handLeft, handRight }
                                                          where hand.IsActive
                                                          orderby hand.Joint.Position.Z
                                                          select hand;
                this.HandMaster = master.FirstOrDefault();

                this.SetNewHandMasterPosition();
            }
        }

        /// <summary>
        ///   The set new hand master position.
        /// </summary>
        private void SetNewHandMasterPosition()
        {
            if (this.HandMaster == null)
            {
                if (this.lastHandMasterPosition != null)
                {
                    this.SetNewHandMasterPosition(null);
                }
            }
            else if (this.lastHandMasterPosition != this.HandMaster.Point)
            {
                this.SetNewHandMasterPosition(this.HandMaster.Point);
            }
        }

        /// <summary>
        ///   The set new hand master position.
        /// </summary>
        /// <param name = "point">
        ///   The point.
        /// </param>
        private void SetNewHandMasterPosition(Point? point)
        {
            this.lastHandMasterPosition = point;
            this.OnMasterPositionChanged(this.lastHandMasterPosition);
        }

        private void helloTracker_HelloGestureDetected(object sender, EventArgs e)
        {
            this.KinectService.InteractionManager.CurrentInteractionMode = Interactions.Kinect;
        }

        #endregion
    }
}